#include "sys.h"
//float 
//	Kp=1,//340*0.6
//  Ki=0.1,
//	Kd=0;//1.2*0.6
int pwm;
struct _pid pid;
int Vertical(float Med,float Angle)
{
	static int error_Now,error_Pre;
	static int erroe_Iteg;
	int PWM_out;
	erroe_Iteg += error_Now;
	if(erroe_Iteg > 500)
	erroe_Iteg=500;
	error_Pre = error_Now;
	error_Now = Angle-Med;
	PWM_out = pid.Kp*error_Now + pid.Ki*erroe_Iteg + pid.Kd*(error_Now-error_Pre);
	return PWM_out;
}
